#include "motor.h"
/*
    pwm初始化用于控制舵机和直流电机，

    电机
    INA1——M0P0——PH0          INA2——M0P1——PH1
    INB1——M0P2——PH2          INB2——M0P3——PH3

     IN1   IN2      功能
    PWM  | 0   | 正转 PWM, 快衰减
    1   | PWM  | 正转 PWM, 慢衰减
    0   | PWM  | 反转 PWM, 快衰减
    PWM  | 1   | 反转 PWM, 慢衰减

    舵机 1 MG996R 180°舵机
    M1PWM3——PG5
    舵机的PWM控制一般需要一个20ms的时基脉冲，该脉冲的高电平部分一般为0.5ms~2.5ms范围内的角度控制脉冲部分。
    以180度角度舵机为例，对应的控制关系是这样的：
                        对于180°												对于360°舵机
    0.5ms--------------	0度；  	2.5% 	（0.5ms高电平+19.5低电平）   625		最大速度正转
    1.0ms--------------	45度； 	5.0% 	（1.0ms高电平+19.0低电平）   1250
    1.5ms--------------	90度； 	7.5% 	（1.5ms高电平+18.5低电平）   1875		停止
    2.0ms--------------	135度； 10% 	（2.0ms高电平+18.0低电平）   2500
    2.5ms--------------	180度； 12.5%	（2.5ms高电平+17.5低电平）   3125		最大速度反转
    注意：
      而对360度舵机：实际相当于无极变速的减速电机，可以控制速度和方向。没有0-360度角度控制的功能。
    控制方式和一般舵机的控制信号相同。是瓦力改装的合适动力源。

      高电平为0.5毫秒~1.5毫秒时，舵机正转(0.5毫秒时正转速度最快(0.4ms也行)，越接近1.5毫秒越慢, 1.5毫秒时舵机停转)，
      高电平为1.5毫秒~ 2.5毫秒时舵机反转(1.5毫秒时舵机停转,越接近2.5毫秒反转的速度越快，2.5毫秒时以最快的速度反转)

    所以：
      入口参数可以通过设定1500 1500×1.25就是1875对应的就是1.5ms。
                   2500×1.25＝3125  2.5ms

    舵机2 TBSK20 数字舵机 范围 0~270°
    500us对应0°   2500对应270°  1500为中位数
    数字舵机中 145°是垂直于水平面  100°是当前云台前倾的最大角度 所以实际上云台的转动角是从100°~270°
    模拟舵机中 0°在最右边，180°在最左边，80°与人垂直
*/
uint16_t myPeriod;

/**
 * @brief Motor_Init
 *
 * 用于控制直流电机的pwm初始化
 *
 */
void MOTOR_Init(void)
{
    SysCtlPWMClockSet(SYSCTL_PWMDIV_64);         // 480/64=1.25M
    SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM0);  // 使能pem外设
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOH); // 使能gpio端口

    // gpio口的复用 A右侧电机
    GPIOPinConfigure(GPIO_PH0_M0PWM0); // INA1
    GPIOPinConfigure(GPIO_PH1_M0PWM1); // INA2
    GPIOPinTypePWM(GPIO_PORTH_BASE, GPIO_PIN_0);
    GPIOPinTypePWM(GPIO_PORTH_BASE, GPIO_PIN_1);

    // gpio口的复用 B左侧电机B
    GPIOPinConfigure(GPIO_PH2_M0PWM2); // INB1
    GPIOPinConfigure(GPIO_PH3_M0PWM3); // INB2
    GPIOPinTypePWM(GPIO_PORTH_BASE, GPIO_PIN_2);
    GPIOPinTypePWM(GPIO_PORTH_BASE, GPIO_PIN_3);

    // pwm的配置 采用中央对齐计数，并且不同步,使用pwm定时器0
    PWMGenConfigure(PWM0_BASE, PWM_GEN_0, PWM_GEN_MODE_UP_DOWN | PWM_GEN_MODE_NO_SYNC);
    PWMGenConfigure(PWM0_BASE, PWM_GEN_1, PWM_GEN_MODE_UP_DOWN | PWM_GEN_MODE_NO_SYNC);

    myPeriod = MOTOR_PERIDO_0_8MS;                   // 设置pwm定时器的重装载值为1000
    PWMGenPeriodSet(PWM0_BASE, PWM_GEN_0, myPeriod); // 1000重装载
    PWMGenPeriodSet(PWM0_BASE, PWM_GEN_1, myPeriod); // 1000重装载

    // 使能pwm定时器0和1
    PWMGenEnable(PWM0_BASE, PWM_GEN_0);
    PWMGenEnable(PWM0_BASE, PWM_GEN_1);
    PWMOutputState(PWM0_BASE, PWM_OUT_0_BIT | PWM_OUT_1_BIT, true); // 使能pwm输出
    PWMOutputState(PWM0_BASE, PWM_OUT_2_BIT | PWM_OUT_3_BIT, true); // 使能pwm输出
}
/**
 * @brief SetMotorPWM_A
 * 右侧电机A的运行，正转，反转以及运行
 * @param pwm
 * @param weaken  ture快衰减 false 慢衰减
 */
void SetMotorPWM_R(int16_t pwm)
{
#if MOTOR_WEAKEN_CTL
    if (pwm >= 0) // 正转
    {
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, pwm);
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, 0);
    }
    else if (pwm <= 0) // 反转
    {
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, 0);
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, -pwm);
    }
#else
    if (pwm >= 0) // 正转
    {
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, 1000);
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, pwm);
    }
    else if (pwm <= 0) // 反转
    {
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, -pwm);
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, 1000);
    }

#endif
}

/**
 * @brief SetMotorPWM_B
 * 控制左侧电机B，正转，反转以及运行
 * @param pwm
 * @param weaken  ture快衰减 false 慢衰减
 */
void SetMotorPWM_L(int16_t pwm)
{
#ifdef MOTOR_WEAKEN_CTL // 快衰减
    if (pwm >= 0)       // 正转
    {
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2, pwm);
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3, 0);
    }
    else if (pwm <= 0) // 反转
    {
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2, 0);
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3, -pwm);
    }
#else // 慢衰减
    if (pwm >= 0) // 正转
    {
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2, 1000);
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3, pwm);
    }
    else if (pwm <= 0) // 反转
    {
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2, -pwm);
        PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3, 1000);
    }

#endif
}

/**
 * @brief Motor_Stop
 *
 * 两个电机停止运转
 *
 */
void Motor_Stop(void)
{
    PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, 0);
    PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, 0);
    PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2, 0);
    PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3, 0);
}

/**
 * @brief SERVOS_Init
 *
 * 用于初始化MG90R舵机
 *
 */
void SERVOS_Init(void)
{
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
    // Unlock PF0 so we can change it to a GPIO input
    // Once we have enabled (unlocked) the commit register then re-lock it
    // to prevent further changes.  PF0 is muxed with NMI thus a special case.
    HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = GPIO_LOCK_KEY;
    HWREG(GPIO_PORTF_BASE + GPIO_O_CR) |= GPIO_PIN_0;
    HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = 0;

    // 使能x轴舵机输出  M1PWM3
    SysCtlPWMClockSet(SYSCTL_PWMDIV_64);         // 480/64=1.25M
    SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM1);  // 使能pem外设
    SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG); // 使能gpio端口
    // gpio口的复用
    GPIOPinConfigure(GPIO_PG5_M1PWM3);
    GPIOPinTypePWM(GPIO_PORTG_BASE, GPIO_PIN_5);
    // pwm的配置 采用中央对齐计数，并且不同步
    PWMGenConfigure(PWM1_BASE, PWM_GEN_1, PWM_GEN_MODE_UP_DOWN | PWM_GEN_MODE_NO_SYNC);
    myPeriod = SERVOS_PERIDO_20MS;                   // 设置pwm定时器的重装载值
    PWMGenPeriodSet(PWM1_BASE, PWM_GEN_1, myPeriod); // 20ms周期的脉冲
    PWMGenEnable(PWM1_BASE, PWM_GEN_1);
    PWMOutputState(PWM1_BASE, PWM_OUT_3_BIT, true); // 使能pwm输出

    // 使能y轴舵机输出 M1PWM2
    GPIOPinConfigure(GPIO_PG4_M1PWM2);
    GPIOPinTypePWM(GPIO_PORTG_BASE, GPIO_PIN_4);
    // pwm的配置 采用中央对齐计数，并且不同步
    PWMGenConfigure(PWM1_BASE, PWM_GEN_1, PWM_GEN_MODE_UP_DOWN | PWM_GEN_MODE_NO_SYNC);
    myPeriod = SERVOS_PERIDO_20MS;                   // 设置pwm定时器的重装载值
    PWMGenPeriodSet(PWM1_BASE, PWM_GEN_1, myPeriod); // 20ms周期的脉冲
    PWMGenEnable(PWM1_BASE, PWM_GEN_1);
    PWMOutputState(PWM1_BASE, PWM_OUT_2_BIT, true); // 使能pwm输出
}

/**
 * @brief Servos_AngleCtl
 * 舵机的X轴角度控制
 * @param Angle 0~180
 */
void Servos_AngleCtl_X(uint16_t Angle)
{
    SetServosPWM_X(500 + Angle * 2000 / 180);
}
/**
 * @brief Servos_AngleCtl
 * 舵机的Y轴角度控制
 * @param Angle 0~270
 */
void Servos_AngleCtl_Y(uint16_t Angle)
{
    SetServosPWM_Y(500 + Angle * 2000 / 270);
}

/**
 * @brief Set the Servos P W M object
 * 用于设置x轴舵机的转速
 * @param pwm  500~2500
 */
void SetServosPWM_X(uint16_t pwm)
{
    PWMPulseWidthSet(PWM1_BASE, PWM_OUT_3, pwm * 1.25);
}

/**
 * @brief Set the Servos P W M object
 * 用于设置y轴舵机的转速
 * @param pwm  500~2500
 */
void SetServosPWM_Y(uint16_t pwm)
{
    PWMPulseWidthSet(PWM1_BASE, PWM_OUT_2, pwm * 1.25);
}
